#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Gyro,           sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    FlipperPot,     sensorPotentiometer)
#pragma config(Sensor, in4,    Auton_Pot,      sensorPotentiometer)
#pragma config(Sensor, dgtl1,  JawChanger,     sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           RightBaseBack, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,           LeftArm,       tmotorVex393, openLoop)
#pragma config(Motor,  port5,           TheJaw,        tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           FlipperMotorLeft, tmotorVex269, openLoop, reversed, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port7,           FlipperMotorRight, tmotorVex269, openLoop, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port8,           LeftBaseBack,  tmotorVex393, openLoop)
#pragma config(Motor,  port9,           turntablemotor, tmotorVex393, openLoop)
#pragma config(Motor,  port10,          RightBaseFront, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

// Pot Values
int Trough = 2222;
int HighGoal = 2222; // "
int HighLimit = 5000; // Placeholder for the moment...
int LowLimit = 1080; // "
int Range0 = 5000;
int Range1 = 2000;
int Range2 = 1000;
int Range3 = 0;

// Arm Commands
bool goup = true;
bool godown = true;
#define FlipperToFloor 2560
#define FlipperScore 833
#define FlipperScoreHeavy -100
#define FlipperUp 100
#define FlipperHoldHeavy 25
#define FlipperHoldLight 10
#define FlipperFast 127
#define FlipperSlow 60
#define FlipperMedium 90

task MulBat()
{
	while(1)
	{
		if(SensorValue(ArmPot) >= HighGoal)
		{
			//        180 = Tempo
			//          5 = Default octave
			//    Quarter = Default note length
			//        10% = Break between notes
			//
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  587,   30); wait1Msec( 333);  // Note(D)
			PlayTone(    0,   15); wait1Msec( 167);  // Note(Rest, Duration(Eighth))
			PlayTone(  622,   15); wait1Msec( 167);  // Note(D#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  698,   15); wait1Msec( 167);  // Note(F, Duration(Eighth))
			PlayTone(    0,   15); wait1Msec( 167);  // Note(Rest, Duration(Eighth))
			PlayTone(  698,   30); wait1Msec( 333);  // Note(F)
			return;
		}
	}
}

// LCD buttons
const short leftButton = 1;
const short centerButton = 2;
const short rightButton = 4;

void flippergodown1()
{
	while (SensorValue [FlipperPot] > FlipperToFloor)
	{
		motor(FlipperMotorLeft) = -63;
		motor(FlipperMotorRight) = -63;
	}


	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;


}
void flipperup1()
{
	//jaws will be open
	//aFTER 45 DEGREES CLOSE JAWS
	while (SensorValue [FlipperPot] < 3641)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
		if (SensorValue [FlipperPot] > 3641)
		{
			break;
		}
	}
	wait1Msec(1000);
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	motor(TheJaw) = -127;
	wait1Msec(500);
	motor(TheJaw) = -10;
}

void flippertotrough1()
{
	// first reead the pot on flipper into a local variable
	// keep movimg the flliper down until it reaCHES  the "trough" pot setting
	// once it reaches the trough setting - quit moving the flipper (open jaws?)
	while (SensorValue[FlipperPot] > FlipperScore)
	{
		motor(FlipperMotorRight) = -83;
		motor(FlipperMotorLeft) = -83;
	}

	{
		motor(FlipperMotorLeft) = 0;
		motor(FlipperMotorRight) = 0;
		motor(TheJaw) = 127;
		wait1Msec(1000);
		motor(TheJaw) = 0;
	}
}



void ScoreFive1(bool bArmsAlreadyUp)
{
	bool go = true;

	if (bArmsAlreadyUp)
	{
		motor[TheJaw] = -127;
		wait1Msec(1000);
		motor[TheJaw] = -30;
		//drive forward
		bool go = true;
		while(go == true)
		{
			motor[RightBaseFront] = 127;
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront] = 127;
			motor[LeftBaseBack] = 127;
			wait1Msec(1100);
			go = false;
		}
		motor[RightBaseFront] = 0;
		motor[RightBaseBack] = 0;
		motor[LeftBaseFront] = 0;
		motor[LeftBaseBack] = 0;
	}
	else
	{
		flippergodown1();
		//drive forward
		while(go == true)
		{
			motor[RightBaseFront] = 127;
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront] = 127;
			motor[LeftBaseBack] = 127;
			wait1Msec(1200);//1150
			go = false;
		}
		motor[RightBaseFront] = 0;
		motor[RightBaseBack] = 0;
		motor[LeftBaseFront] = 0;
		motor[LeftBaseBack] = 0;

		/*motor(FlipperMotorLeft) = 120;
		motor(FlipperMotorRight) = 120;
		wait1Msec(1600);
		motor[TheJaw] = -127;
		wait1Msec(1000);
		motor[TheJaw] = -10;
		motor(FlipperMotorLeft) = 10;
		motor(FlipperMotorRight) = 10;*/
		flipperup1();

		//part 3 raise arms to trough
		while(SensorValue(ArmPot) < Trough)
		{
			motor(RightArm) = 127;
			motor(LeftArm) = 127;
		}
		motor(RightArm) = 10;
		motor(LeftArm) = 10;
		go = true;
		//part 1 drive forward
		while(go == true)
		{
			motor[RightBaseFront] = 127;
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront] = 127;
			motor[LeftBaseBack] = 127;
			wait1Msec(270);//220
			go = false;
		}
		motor[RightBaseFront] = 0;
		motor[RightBaseBack] = 0;
		motor[LeftBaseFront] = 0;
		motor[LeftBaseBack] = 0;
	}
	flippertotrough1();
	//openjaws();
	//motor[TheJaw] = 127;
	//wait1Msec(700);
	//motor[TheJaw] = 10;

	go = true;
	//part drive back
	while(go == true)
	{
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = -127;
		motor[LeftBaseBack] = -127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void closejaws()
{
	motor(TheJaw) = -127;
}

void openjaws()
{
	motor(TheJaw) = 127;
}

void flippergodown()
{
	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(1000);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}

void flippertotrough()
{
	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(4000);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}


void flipperup()
{
	motor(FlipperMotorLeft) = 127;
	motor(FlipperMotorRight) = 127;
	wait1Msec(2000);
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
}


void ClearEncoders()
{
	nMotorEncoder[RightBaseBack] = 0;
	nMotorEncoder[RightBaseFront] = 0;
	nMotorEncoder[LeftBaseBack] = 0;
	nMotorEncoder[LeftBaseFront] = 0;
}


//Wait for Press--------------------------------------------------
void waitForPress()
{
	while(nLCDButtons == 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

//Wait for Release------------------------------------------------
void waitForRelease()
{
	while(nLCDButtons != 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

///////////////////////////////////////Start Autonomous Voids//////////////////////////////////////////////////////////



void Drive()
{
	bool go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(1000);
		go = false;
	}
	go = true;
	//part 2 drive backward
	while(go == true)
	{
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = -127;
		motor[LeftBaseBack] = -127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void ScoreFive()
{
	bool go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(700);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	//part 2 turn 90 degrees
	//While the absolute value of the gyro is less than the desired rotation...
	while(SensorValue[Gyro] > -650)
	{
		//...continue turning
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseFront]  = 127;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	flippergodown();
	//part 1 drive forward
	go = true;
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	closejaws();
	flipperup();
	//part 3 raise arms to trough
	while(SensorValue(ArmPot) < Trough)
	{
		motor(RightArm) = 127;
		motor(LeftArm) = 127;
	}
	motor(RightArm) = 10;
	motor(LeftArm) = 10;
	go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(200);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	flippertotrough();
	openjaws();
	wait1Msec(500);

	wait1Msec(300);
	go = true;
	//part drive back
	while(go == true)
	{
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = -127;
		motor[LeftBaseBack] = -127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void ScoreTrough()
{
	bool go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(600);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	//part 2 turn 90 degrees
	//While the absolute value of the gyro is less than the desired rotation...
	while(SensorValue[Gyro] > -800)
	{
		//...continue turning
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseFront]  = 127;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	//part 3 raise arms to trough
	while(SensorValue(ArmPot) < Trough)
	{
		motor(RightArm) = 127;
		motor(LeftArm) = 127;
	}
	motor(RightArm) = 5;
	motor(LeftArm) = 5;
	go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	flippergodown();
	go = true;
	//part drive back
	while(go == true)
	{
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = -127;
		motor[LeftBaseBack] = -127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	flipperup();
	//part  raise arms to floor
	while(SensorValue(ArmPot) > LowLimit)
	{
		motor(RightArm) = -63;
		motor(LeftArm) = -63;
	}
	motor(RightArm) = 5;
	motor(LeftArm) = 5;
}

void Fuse()
{
	bool go = true;
	//part 1 drive forward

	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(800);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;


	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(5000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void flipperdown()
{
	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(600);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}


///////////////////////////////////////Start User Control Voids/////////////////////////////////////////////////////////
// The controls for the user control base.  It has PID in it (nMotorPIDSpeedCtrl[motorX] = mtrSpeedReg in #pragma config)
// as now doesn't have a ramp motor.
void base()
{
	if(vexRT(Btn6U) == 1)
	{
		motor(LeftBaseBack) = vexRT(Ch3);
		motor(LeftBaseFront) = vexRT(Ch3);
		motor(RightBaseBack) = vexRT(Ch2);
		motor(RightBaseFront) = vexRT(Ch2);
	}
	else
	{
		motor(LeftBaseBack) = vexRT(Ch3)/2;
		motor(LeftBaseFront) = vexRT(Ch3)/2;
		motor(RightBaseBack) = vexRT(Ch2)/2;
		motor(RightBaseFront) = vexRT(Ch2)/2;
	}
}

void UArms()
{
	if(vexRT(Btn8UXmtr2) == 1)
	{
		goup = true;
		while(goup == true)
		{
			motor(RightArm) = 127;
			motor(LeftArm) = 127;
			if(SensorValue(ArmPot) == Trough || SensorValue(ArmPot) ==  HighGoal || SensorValue(ArmPot) >= HighLimit)
			{
				goup = false;
				motor(RightArm) = 0;
				motor(LeftArm) = 0;
			}
		}
	}

	motor[RightArm] = vexRT(Ch2Xmtr2)+5; // Lifter Motor 1
	motor[LeftArm] = vexRT(Ch2Xmtr2)+5; // Lifter Motor 2

	if(vexRT(Btn8DXmtr2) == 1)
	{
		godown = true;
		while(godown == true)
		{
			motor(RightArm) = -127;
			motor(LeftArm) = -127;
			if(SensorValue(ArmPot) == Trough || SensorValue(ArmPot) ==  HighGoal || SensorValue(ArmPot) <= LowLimit)
			{
				godown = false;
				motor(RightArm) = 0;
				motor(LeftArm) = 0;
			}
		}
	}
}

void JawsOfDeath()
{
	if(SensorValue(JawChanger) == 1)
	{
		if(vexRT[Btn5U] == 1)
		{
			motor[TheJaw] = 127;
		}
		if(vexRT[Btn5D] == 1)
		{
			motor[TheJaw] = -127;
		}
		if(vexRT[Btn5U] == 0 && vexRT[Btn5D] == 0)
		{
			motor[TheJaw] = 0;
		}
	}

	if(SensorValue(JawChanger) == 0)
	{
		if(vexRT[Btn6UXmtr2] == 1)
		{
			motor[TheJaw] = 127;
		}
		if(vexRT[Btn6DXmtr2] == 1)
		{
			motor[TheJaw] = -127;
		}
		if(vexRT[Btn6UXmtr2] == 0 && vexRT[Btn6DXmtr2] == 0)
		{
			motor[TheJaw] = 0;
		}
	}
}

void Flipper()
{
	motor[FlipperMotorLeft] = vexRT(Ch3Xmtr2);
	motor[FlipperMotorRight] = vexRT(Ch3Xmtr2);
}

void turntable()
{
	if(vexRT[Btn5UXmtr2] == 1)
	{
		motor[turntablemotor] = 127;
	}
	if(vexRT[Btn5DXmtr2] == 1)
	{
		motor[turntablemotor] = -127;
	}
	if(vexRT[Btn5UXmtr2] == 0 && vexRT[Btn5DXmtr2] == 0)
	{
		motor[turntablemotor] = 10;
	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
//////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;
	SensorType[in1] = sensorNone;
	wait1Msec(1000);
	//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
	SensorType[in1] = sensorGyro;
	wait1Msec(2000);

}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	bLCDBacklight = true; // have LCD backlight on
	clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
	clearLCDLine(1);                                            // Clear line 2 (1) of the LCD
	//Declare count variable to keep track of our choice
	int count = 0;

	//------------- Beginning of User Interface Code ---------------
	//Clear LCD
	clearLCDLine(0);
	clearLCDLine(1);
	while(1)
	{
		//Loop while center button is not pressed
		//Switch case that allows the user to choose from 4 different options
		switch(count){
		case 0:
			//Display first choice
			ClearEncoders();
			if(SensorValue(Auton_Pot) > Range1 && SensorValue(Auton_Pot) < Range0)
			{
				ClearEncoders();
				displayLCDCenteredString(0, "ScoreFive1");
				ScoreFive1(false);
				wait10Msec(5000);
			}
			if(SensorValue(Auton_Pot) > Range2 && SensorValue(Auton_Pot) < Range1)
			{
				ClearEncoders();
				displayLCDCenteredString(0, "ScoreFive");
				ScoreTrough();
			}
			if(SensorValue(Auton_Pot) > Range3 && SensorValue(Auton_Pot) < Range2)
			{
				ClearEncoders();
				displayLCDCenteredString(0, "Fuse");
				Fuse();
			}
			//------------- End of Robot Movement Code -----------------------
		}
	}
}


/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	StartTask(MulBat);
	bLCDBacklight = true; // have LCD backlight on
	string mainBattery, backupBattery;
	//Clear the encoders associated with the left and right motors
	nMotorEncoder[RightBaseBack] = 0;
	nMotorEncoder[LeftBaseBack] = 0;
	nMotorEncoder[RightBaseFront] = 0;
	nMotorEncoder[LeftBaseFront] = 0;

	// Run Forever
	while(1)
	{

		// Displays the battery level on the LCD
		clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
		clearLCDLine(1);                                            // Clear line 2 (1) of the LCD

		//Display the Primary Robot battery voltage
		displayLCDString(0, 0, "Primary: ");
		sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
		displayNextLCDString(mainBattery);

		//Display the Backup battery voltage
		displayLCDString(1, 0, "Backup: ");
		sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V');    //Build the value to be displayed
		displayNextLCDString(backupBattery);

		//Short delay for the LCD refresh rate
		wait1Msec(100);
		// Run the base program
		base();
		// run the arms program
		UArms();
		// run the "Jaws" program
		JawsOfDeath();
		// run the flipper program
		Flipper();
		// run the turntable program
		turntable();
		// rhett - you cant put this here because it is in the infinite loop - it wil keep getting called over and over and over and over and over and over...
		//PlaySoundFile("nyan.wav"); // must upload via file manager
	}
}
// end of code!!!!!!!!!!
